#include "RgbUltrasonic.h"

RgbUltrasonic::RgbUltrasonic(byte signal_pin, byte rgb_pin, byte servo_pin)
{
  this->ServoPIN(servo_pin);
  SignalPin = signal_pin;
  RgbPin = rgb_pin;
  mRgb = new RGBLed(RgbPin, 6);
  ruServoExt = new ServoExt();
}
RgbUltrasonic::RgbUltrasonic(byte signal_pin, byte rgb_pin)
{
  SignalPin = signal_pin;
  RgbPin = rgb_pin;
  mRgb = new RGBLed(RgbPin, 6);
  ruServoExt = new ServoExt();
}

void RgbUltrasonic::ServoPIN(byte servo_pin)
{
  ServoPin = servo_pin;
  //    pinMode(ServoPin,OUTPUT);
}
uint16_t RgbUltrasonic::GetUltrasonicFrontDistance()
{
  unsigned long Time_Echo_us = 0;
  pinMode(SignalPin, OUTPUT);
  digitalWrite(SignalPin, LOW);
  delayMicroseconds(2);
  digitalWrite(SignalPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(SignalPin, LOW);
  pinMode(SignalPin, INPUT);
  Time_Echo_us = pulseIn(SignalPin, HIGH);
  if ((Time_Echo_us < 60000) && (Time_Echo_us > 1)) {
    FrontDistance = Time_Echo_us / 58.00;
    return FrontDistance;
  }
  return 0;
}

uint16_t RgbUltrasonic::GetUltrasonicDistance(void) {

  return GetUltrasonicFrontDistance();
}

uint16_t RgbUltrasonic::GetUltrasonicLeftDistance()
{
  SetServoDegree(175);
  LeftDistance = GetUltrasonicFrontDistance();
  SetServoDegree(90);
  return LeftDistance;
}

uint16_t RgbUltrasonic::GetUltrasonicRightDistance()
{
  SetServoDegree(5);
  RightDistance = GetUltrasonicFrontDistance();
  SetServoDegree(90);
  return RightDistance;
}

void RgbUltrasonic::SetServoBaseDegree(uint8_t base)
{
  ServoBaseDegree = base;
}

void RgbUltrasonic::SetServoDegree(int angle)
{
//  int Degree = angle;
//  int servo_degree;
//  if (Degree > 360) {
//    return;
//  }
//  if (Degree == 90 || Degree == 270) {
//    servo_degree = ServoBaseDegree;
//  } else if (Degree >= 0 && Degree <= 180) {
//    servo_degree = ServoBaseDegree - 90 + Degree;   // 180-degree-diff
//  }
//  float pulsewidth = (servo_degree * 11) + 350;
//  Serial.print("degree val: ");
//  Serial.print(servo_degree);
//  Serial.print(", pulsewidth val: ");
//  Serial.println(pulsewidth);
//
//  for (int i = 0; i < 80; i++) {
//    digitalWrite(ServoPin, HIGH);   //Set the servo interface level to high
//    delayMicroseconds(pulsewidth);  //The number of microseconds of the delay pulse width value
//    digitalWrite(ServoPin, LOW);    //Set the servo interface level to low
//    delayMicroseconds(20000 - pulsewidth);
//  }
//  delay(250);

    ruServoExt->SetServoDegree(ServoPin, angle, 500);
//    ruServoExt->detach(500);
}

void RgbUltrasonic::SetRgbColor(E_RGB_INDEX index, long Color)
{
  if (index == E_RGB_ALL) {
    mRgb->setColor(0, Color);
  } else if (index == E_RGB_RIGHT) {
    mRgb->setColor(1, Color);
    mRgb->setColor(2, Color);
    mRgb->setColor(3, Color);
  } else if (index == E_RGB_LEFT) {
    mRgb->setColor(4, Color);
    mRgb->setColor(5, Color);
    mRgb->setColor(6, Color);
  }
  mRgb->show();
}

void RgbUltrasonic::SetRgbIndexColor(int start_index, int end_index, long Color)
{
  for (byte i = start_index; i <= end_index; i++) {
    mRgb->setColor(i, Color);
  }
  mRgb->show();
}

void RgbUltrasonic::SetRgbEffect(E_RGB_INDEX index, long Color, uint8_t effect = E_EFFECT_NONE)
{
  byte start = 1, end = 6;
  if (index == E_RGB_RIGHT) {
    start = 4;
    end = 6;
  } else if (index == E_RGB_LEFT) {
    start = 1;
    end = 3;
  }

  switch ((E_RGB_EFFECT)effect) {
    case E_EFFECT_NONE:
      //Serial.println("E_EFFECT_NONE");
      SetRgbIndexColor(start, end, Color);
      break;
    case E_EFFECT_BREATHING:
      //Serial.println("E_EFFECT_BREATHING");
      for (int i = 0; i < 256; i++) {
        mRgb->setBrightness(i);
        SetRgbIndexColor(start, end, Color);
        //delay((255 - i)/2);
        delay((i < 20) ? 60 : (255 / i));
      }
      for (int i = 255; i >= 0; i--) {
        mRgb->setBrightness(i);
        SetRgbIndexColor(start, end, Color);
        delay((i < 20) ? 60 : (255 / i));
      }
      mRgb->setBrightness(255);
      break;
    case E_EFFECT_ROTATE:
      //Serial.println("E_EFFECT_ROTATE");
      for (byte i = 0; i < 4; i++) {
        mRgb->setColor(start, Color);
        mRgb->setColor(start + 1, 0);
        mRgb->setColor(start + 2, 0);
        if (index == E_RGB_ALL) {
          mRgb->setColor(end - 2, Color);
          mRgb->setColor(end - 1, 0);
          mRgb->setColor(end, 0);
        }
        mRgb->show();
        delay(150);
        mRgb->setColor(start, 0);
        mRgb->setColor(start + 1, Color);
        mRgb->setColor(start + 2, 0);
        if (index == E_RGB_ALL) {
          mRgb->setColor(end - 2, 0);
          mRgb->setColor(end - 1, Color);
          mRgb->setColor(end, 0);
        }
        mRgb->show();
        delay(150);
        mRgb->setColor(start, 0);
        mRgb->setColor(start + 1, 0);
        mRgb->setColor(start + 2, Color);
        if (index == E_RGB_ALL) {
          mRgb->setColor(end - 2, 0);
          mRgb->setColor(end - 1, 0);
          mRgb->setColor(end, Color);
        }
        mRgb->show();
        delay(150);
      }
      SetRgbIndexColor(1, 6, 0);
      break;
    case E_EFFECT_FLASH:
      //Serial.println("E_EFFECT_FLASH");
      for (byte i = 0; i < 6; i++) {
        SetRgbIndexColor(start, end, Color);
        delay(150);
        SetRgbIndexColor(start, end, 0);
        delay(150);
      }
      break;
  }
}
